#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// 窗口标题常量
static const std::string OPENCV_WINDOW_COLOR = "Image color window";
static const std::string OPENCV_WINDOW_DEPTH = "Image depth window";

/**
 * 图像转换器类，用于处理颜色和深度图像
 */
class ImageConverter
{
  ros::NodeHandle nh_;          // ROS节点句柄
  image_transport::ImageTransport it_;  // 图像传输对象
  image_transport::Subscriber image_sub_color;  // 颜色图像订阅者
  image_transport::Subscriber image_sub_depth;  // 深度图像订阅者
  image_transport::Publisher image_pub_color;  // 颜色图像发布者
  image_transport::Publisher image_pub_depth;  // 深度图像发布者

public:
  /**
   * 构造函数，初始化图像订阅者和发布者，创建OpenCV窗口
   */
  ImageConverter()
    : it_(nh_)
  {
    // 订阅颜色图像主题
    image_sub_color = it_.subscribe("/camera/color/image_raw", 1, &ImageConverter::imageCb, this);
    // 订阅深度图像主题
    image_sub_depth = it_.subscribe("/camera/depth/image_rect_raw", 1, &ImageConverter::depthCb, this);
    // 发布处理后的颜色图像
    image_pub_color = it_.advertise("/image_converter/output_video/color", 1);
    // 发布处理后的深度图像
    image_pub_depth = it_.advertise("/image_converter/output_video/depth", 1);

    // 创建OpenCV窗口
    cv::namedWindow(OPENCV_WINDOW_COLOR);
    cv::namedWindow(OPENCV_WINDOW_DEPTH);
  }

  /**
   * 析构函数，关闭OpenCV窗口
   */
  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW_COLOR);
    cv::destroyWindow(OPENCV_WINDOW_DEPTH);
  }

  /**
   * 颜色图像回调函数，处理接收到的颜色图像消息
   * @param msg 颜色图像消息
   */
  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      // 将ROS图像消息转换为OpenCV图像
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // 在图像上绘制示例圆圈
    if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // 更新GUI窗口
    cv::imshow(OPENCV_WINDOW_COLOR, cv_ptr->image);
    cv::waitKey(3);

    // 发布处理后的图像
    image_pub_color.publish(cv_ptr->toImageMsg());
  }

  /**
   * 深度图像回调函数，处理接收到的深度图像消息
   * @param msg 深度图像消息
   */
  void depthCb(const sensor_msgs::ImageConstPtr &msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      // 将ROS图像消息转换为OpenCV图像
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // 在图像上绘制示例圆圈
    if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // 更新GUI窗口
    cv::imshow(OPENCV_WINDOW_DEPTH, cv_ptr->image);
    cv::waitKey(3);

    // 发布处理后的图像
    image_pub_depth.publish(cv_ptr->toImageMsg());
  }
};

/**
 * 主函数，初始化ROS节点和图像转换器对象
 * @param argc 命令行参数个数
 * @param argv 命令行参数数组
 * @return 程序退出状态
 */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}